#include <signal.h>
#include <stdlib.h>
#include <iostream>
#include <algorithm>
#include <fstream>
#include <chrono>
#include <ctime>
#include <sstream>

#include <condition_variable>

#include <opencv2/core/core.hpp>
#include <boost/lockfree/spsc_queue.hpp>

#include "System.h"

using namespace std;

bool b_continue_session = true;

void exit_loop_handler(int s){
    cout << "Finishing session" << endl;
    b_continue_session = false;
    exit(1);
}

class Application
{
private:
    std::shared_ptr<ORB_SLAM3::System> p_system;
    boost::lockfree::spsc_queue<std::pair<double, cv::Mat>> image_queue_ {10};
    float imageScale = 0.0;
    int count_im_buffer = 0; // count dropped frames

    std::shared_ptr<std::thread> p_work_thread;

public:
    Application(/* args */);
    ~Application();

    void Init(const string &strVocFile, const string &strSettingsFile, const string &strSequence = std::string());
    void Quit();
    void insertImage(cv::Mat image, double timestamp);

protected:
    void WorkTask();
};

Application::Application(/* args */)
{
}

Application::~Application()
{
}

void Application::Init(const string &strVocFile, const string &strSettingsFile, const string &strSequence)
{
    p_system = std::make_shared<ORB_SLAM3::System>(strVocFile, strSettingsFile, ORB_SLAM3::System::MONOCULAR, true, 0, strSequence);
    imageScale = p_system->GetImageScale();

    p_work_thread = std::make_shared<std::thread>(&Application::WorkTask, this);
    p_work_thread->detach();
}

void Application::Quit()
{
    if (p_system != nullptr) {
        p_system->Shutdown();
    }
}

void Application::insertImage(cv::Mat image, double timestamp)
{
    image_queue_.push(std::pair<double, cv::Mat>(timestamp, image));
}

void Application::WorkTask()
{
    double timestamp;
    cv::Mat im;

    double t_resize = 0.f;
    double t_track = 0.f;

    printf("%s start.\n", __FUNCTION__);
    while (!p_system->isShutDown())
    {
        if (image_queue_.empty()) {
            std::this_thread::sleep_for(std::chrono::milliseconds(5));
            continue;
        }
        
        std::pair<double, cv::Mat> image_item = image_queue_.front();
        image_queue_.pop();

        printf("deal with image_item timestamp: %.6f.\n", image_item.first);

        cv::Mat imCV = image_item.second;

        std::chrono::steady_clock::time_point time_Start_Process = std::chrono::steady_clock::now();

        if(count_im_buffer>1)
            cout << count_im_buffer -1 << " dropped frs\n";
        count_im_buffer = 0;

        timestamp = image_item.first;
        im = imCV.clone();

        if(imageScale != 1.f)
        {
#ifdef REGISTER_TIMES
    #ifdef COMPILEDWITHC11
            std::chrono::steady_clock::time_point t_Start_Resize = std::chrono::steady_clock::now();
    #else
            std::chrono::monotonic_clock::time_point t_Start_Resize = std::chrono::monotonic_clock::now();
    #endif
#endif
            int width = im.cols * imageScale;
            int height = im.rows * imageScale;
            cv::resize(im, im, cv::Size(width, height));

#ifdef REGISTER_TIMES
    #ifdef COMPILEDWITHC11
            std::chrono::steady_clock::time_point t_End_Resize = std::chrono::steady_clock::now();
    #else
            std::chrono::monotonic_clock::time_point t_End_Resize = std::chrono::monotonic_clock::now();
    #endif
            t_resize = std::chrono::duration_cast<std::chrono::duration<double,std::milli> >(t_End_Resize - t_Start_Resize).count();
            p_system->InsertResizeTime(t_resize);
#endif
        }

#ifdef REGISTER_TIMES
    #ifdef COMPILEDWITHC11
        std::chrono::steady_clock::time_point t_Start_Track = std::chrono::steady_clock::now();
    #else
        std::chrono::monotonic_clock::time_point t_Start_Track = std::chrono::monotonic_clock::now();
    #endif
#endif
        // Stereo images are already rectified.
        p_system->TrackMonocular(im, timestamp);
#ifdef REGISTER_TIMES
    #ifdef COMPILEDWITHC11
        std::chrono::steady_clock::time_point t_End_Track = std::chrono::steady_clock::now();
    #else
        std::chrono::monotonic_clock::time_point t_End_Track = std::chrono::monotonic_clock::now();
    #endif
        t_track = t_resize + std::chrono::duration_cast<std::chrono::duration<double,std::milli> >(t_End_Track - t_Start_Track).count();
        p_system->InsertTrackTime(t_track);
#endif
    }
    cout << "System shutdown!\n";
}


int main(int argc, char **argv) {
    if (argc < 3 || argc > 4) {
        cerr << endl
             << "Usage: ./mono_realsense_D435i path_to_vocabulary path_to_settings (trajectory_file_name)"
             << endl;
        return 1;
    }

    string file_name;

    if (argc == 4) {
        file_name = string(argv[argc - 1]);
    }

    struct sigaction sigIntHandler;

    sigIntHandler.sa_handler = exit_loop_handler;
    sigemptyset(&sigIntHandler.sa_mask);
    sigIntHandler.sa_flags = 0;

    sigaction(SIGINT, &sigIntHandler, NULL);
    
    Application application;
    application.Init(argv[1], argv[2], file_name);

    while (b_continue_session) {
        printf("This app is alive...\n");

        std::this_thread::sleep_for(std::chrono::milliseconds(1000));
    }

    application.Quit();
}